#include <gl/glut.h>
#include "surftrack.h"
#include <vec.h>
#include <vector>
#include <windows.h>
#include <stdio.h>
#include <BEMUtil.h>
#include <SolverUtil.h>
#include <EvalUtil.h>


using namespace std;
using namespace eltopo2d;

SurfTrack *surf;

double* u;


void renderSurface()
{

	glColor3d(1, 1, 1);

	glLineWidth(2.0);
	glBegin(GL_LINES);
	for (int i = 0; i < surf->m_mesh.edges.size(); i++) {
		Vec2ui e = surf->m_mesh.edges[i];
		Vec2d a(surf->m_positions[e[0]]);
		Vec2d b(surf->m_positions[e[1]]);

		Vec2d n = surf->get_edge_normal(surf->m_mesh.edges[i]);
		double l = surf->get_edge_length(i);
		Vec2d vn1 = surf->get_vertex_normal(e[0]) * 0.1;
		Vec2d vn2 = surf->get_vertex_normal(e[1]) * 0.1;


		glVertex3d(a[0] + u[e[0] * 2 + 0], a[1] + u[e[0] * 2 + 1], 0);
		glVertex3d(b[0] + u[e[1] * 2 + 0], b[1] + u[e[1] * 2 + 1], 0);

		//glVertex3d(a[0], a[1], 0);
		//glVertex3d(a[0]+vn1[0], a[1]+vn1[1], 0);

		//glVertex3d(b[0], b[1], 0);
		//glVertex3d(b[0]+vn2[0], b[1]+vn2[1], 0);

		//glVertex3d((a[0]+b[0])/2, (a[1]+b[1])/2, 0);
		//glVertex3d((a[0]+b[0])/2+n[0]*0.1, (a[1]+b[1])/2+n[1]*0.1, 0);
	}
	glEnd();

	

	glColor3d(1, 0, 0);
	glPointSize(3);

	glBegin(GL_POINTS);

	for (int i = 0; i < surf->m_positions.size(); i++)
	{
		Vec2d pos = surf->m_positions.at(i);

		glVertex3d(pos[0], pos[1], 0);
	}

	glEnd();

	/*glBegin(GL_POINTS);

	for (int i=0; i<mine->pcs->size(); i++)
	{
	PotentialCollision pc = mine->pcs->at(i);

	Crack2d cc = mine->crack->at(pc.a_i);

	//printf("%d\n", cc.pt_i);

	Vec2d vs = mine->surf->m_positions.at(cc.pt_i);

	Vec2d vd = cc.vel * pc.a_time * 1 + vs;

	//printf("%f %f\n", vd[0], vd[1]);

	//glVertex3d(vd[0], vd[1], 0);
	}

	glEnd();*/

	/*for (int i=0; i<surf->m_positions.size(); i++)
	{
	if (i == -30 || i == -60 || i == -130)
	{
	glPointSize(10);
	glColor3d(0, 0, 1);
	}else{
	glPointSize(3);
	glColor3d(1, 0, 0);
	}

	glBegin(GL_POINTS);

	Vec2d pos = surf->m_positions.at(i);

	glVertex3d(pos[0], pos[1], 0);

	glEnd();
	}*/



}

void reshape(GLint width, GLint height)
{
	glViewport(0, 0, width, height);
	glMatrixMode(GL_PROJECTION);
	glLoadIdentity();
	gluPerspective(65.0, (float)width / height, 1, 1000);
	glMatrixMode(GL_MODELVIEW);
}

void display(void)
{		
	//glClearColor(1.0, 1.0, 1.0, 1.0);
	glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
	// Set up viewing transformation, looking down -Z axis
	glLoadIdentity();
	gluLookAt(0, 0, 6, 0, 0, 0, 0, 1, 0);

	// Render the scene	
	renderSurface();
	
	glutSwapBuffers();

	glutPostRedisplay();
}

void test()
{
	int n = surf->m_mesh.edges.size() * 2;
	int m = surf->m_positions.size() * 2;

	double* b = (double*)malloc(n * sizeof(double));
	double* x = (double*)malloc(n * sizeof(double));
	double* F = (double*)malloc(m * 2 * sizeof(double));
	u = (double*)malloc(m * sizeof(double));

	vectorClear(&x, n);
	assemblyBoundaryVector(surf, &b);

	biCGStab(myMatrixVectorProduct, b, &x, n, 10000, 1e-6);

	evaluateDisplacementField(x, &u);
}

int main(int argc, char** argv)
{
	eltopo2d::SurfTrackInitializationParameters eltopoParams;
	eltopoParams.m_min_edge_length = 0.05;
	eltopoParams.m_max_edge_length = 0.08;
	eltopoParams.m_merge_proximity_epsilon = 0.05;
	eltopoParams.m_max_area_change = 0.1;
	eltopoParams.m_allow_topology_changes = true;
	eltopoParams.m_verbose = false;
	eltopoParams.m_collision_safety = false;
	eltopoParams.m_perform_improvement = true;


	vector<Vec2d> verts;
	vector<Vec2ui> edges;
	vector<double> masses;

	verts.push_back(Vec2d(1, 1));
	verts.push_back(Vec2d(-1, 1));
	verts.push_back(Vec2d(-1, -1));
	verts.push_back(Vec2d(1, -1));
	

	edges.push_back(Vec2ui(0, 1));
	edges.push_back(Vec2ui(1, 2));
	edges.push_back(Vec2ui(2, 3));
	edges.push_back(Vec2ui(3, 0));
	

	masses.push_back(1);
	masses.push_back(1);
	masses.push_back(1);
	masses.push_back(1);
	

	surf = new SurfTrack(verts, edges, masses, eltopoParams);

	surf->improve_mesh();
	surf->improve_mesh();
	surf->rebuild_static_broad_phase();

	test();

	glutInit(&argc, argv);
	glutInitWindowSize(800, 800);
	glutInitDisplayMode(GLUT_RGB | GLUT_DOUBLE | GLUT_DEPTH);
	glutCreateWindow("El Topo 2D");

	glutDisplayFunc(display);
	glutReshapeFunc(reshape);
	//glutIdleFunc(AnimateScene);

	glutMainLoop();

	rand();
	printf("hello world %d \n", rand() % 100);


	system("pause");

	return 1;
}